package edu.tec.mty.core.modules;

import java.util.*;
import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;

import edu.tec.mty.core.modules.models.Position;

public class SelfModel extends Thread {
	
	private double x = 0.0;
	private double y = 0.0;
	private double bodyAngle = 0.0;
	private double neckAngle = 0.0;
	private double side;
	private boolean init = false;
	private HashMap<String, Coordinate> field = new HashMap<String, Coordinate>();
	private HashMap<String, String> flags = new HashMap<String, String>();
	private boolean running = true;
	private final Lock lock = new ReentrantLock();
	private final Condition update = lock.newCondition();
	private boolean updating = false;
	private boolean closing = false;
	private long step = 0;
	private long lastStepUpdated = 0;
	private int accuracy = 0;
	
	class Coordinate {
		
		public double x;
		public double y;
	
		public Coordinate(double x, double y, double side) {
			this.x = x * side;
			this.y = y * side;
		}
	
	}
	
	public SelfModel(String side, int accuracy) {
		this.accuracy = accuracy;
		if (side.equals("left")) {
			this.side = 1.0;
		} else {
			this.side = -1.0;
		}
		//Flags outside the players field
		this.field.put("f t 0",    new Coordinate(  0.0, -39.0, this.side)); // flag t 0
		this.field.put("f t r 10", new Coordinate( 10.0, -39.0, this.side)); // flag t r 10
		this.field.put("f t r 20", new Coordinate( 20.0, -39.0, this.side)); // flag t r 20
		this.field.put("f t r 30", new Coordinate( 30.0, -39.0, this.side)); // flag t r 30
		this.field.put("f t r 40", new Coordinate( 40.0, -39.0, this.side)); // flag t r 40
		this.field.put("f t r 50", new Coordinate( 50.0, -39.0, this.side)); // flag t r 50
		this.field.put("f r t 30", new Coordinate( 57.5, -30.0, this.side)); // flag r t 30
		this.field.put("f r t 20", new Coordinate( 57.5, -20.0, this.side)); // flag r t 20
		this.field.put("f r t 10", new Coordinate( 57.5, -10.0, this.side)); // flag r t 10
		this.field.put("f r 0",    new Coordinate( 57.5,   0.0, this.side)); // flag r 0
		this.field.put("f r b 10", new Coordinate( 57.5,  10.0, this.side)); // flag r b 10
		this.field.put("f r b 20", new Coordinate( 57.5,  20.0, this.side)); // flag r b 20
		this.field.put("f r b 30", new Coordinate( 57.5,  30.0, this.side)); // flag r b 30
		this.field.put("f b r 50", new Coordinate( 50.0,  39.0, this.side)); // flag r b 50
		this.field.put("f b r 40", new Coordinate( 40.0,  39.0, this.side)); // flag r b 40
		this.field.put("f b r 30", new Coordinate( 30.0,  39.0, this.side)); // flag r b 30
		this.field.put("f b r 20", new Coordinate( 20.0,  39.0, this.side)); // flag r b 20
		this.field.put("f b r 10", new Coordinate( 10.0,  39.0, this.side)); // flag r b 10
		this.field.put("f b 0",    new Coordinate(  0.0,  39.0, this.side)); // flag b 0
		this.field.put("f b l 10", new Coordinate(-10.0,  39.0, this.side)); // flag b l 10
		this.field.put("f b l 20", new Coordinate(-20.0,  39.0, this.side)); // flag b l 20
		this.field.put("f b l 30", new Coordinate(-30.0,  39.0, this.side)); // flag b l 30
		this.field.put("f b l 40", new Coordinate(-40.0,  39.0, this.side)); // flag b l 40
		this.field.put("f b l 50", new Coordinate(-50.0,  39.0, this.side)); // flag b l 50
		this.field.put("f l b 30", new Coordinate(-57.5,  30.0, this.side)); // flag l b 30
		this.field.put("f l b 20", new Coordinate(-57.5,  20.0, this.side)); // flag l b 20
		this.field.put("f l b 10", new Coordinate(-57.5,  10.0, this.side)); // flag l b 10
		this.field.put("f l 0",    new Coordinate(-57.5,   0.0, this.side)); // flag l 0
		this.field.put("f l t 10", new Coordinate(-57.5, -10.0, this.side)); // flag l t 10
		this.field.put("f l t 20", new Coordinate(-57.5, -20.0, this.side)); // flag l t 20
		this.field.put("f l t 30", new Coordinate(-57.5, -30.0, this.side)); // flag l t 30
		this.field.put("f t l 50", new Coordinate(-50.0, -39.0, this.side)); // flag t l 50
		this.field.put("f t l 40", new Coordinate(-40.0, -39.0, this.side)); // flag t l 40
		this.field.put("f t l 30", new Coordinate(-30.0, -39.0, this.side)); // flag t l 30
		this.field.put("f t l 20", new Coordinate(-20.0, -39.0, this.side)); // flag t l 20
		this.field.put("f t l 10", new Coordinate(-10.0, -39.0, this.side)); // flag t l 10
		//Flags in the boundary of the player field
		this.field.put("f c",      new Coordinate(  0.0,   0.0, this.side)); // flag c
		this.field.put("f c t",    new Coordinate(  0.0, -34.0, this.side)); // flag c t
		this.field.put("f r t",    new Coordinate( 52.5, -34.0, this.side)); // flag r t
		this.field.put("f r b",    new Coordinate( 52.5,  34.0, this.side)); // flag r b
		this.field.put("f c b",    new Coordinate(  0.0,  34.0, this.side)); // flag c b
		this.field.put("f l b",    new Coordinate(-52.5,  34.0, this.side)); // flag l b
		this.field.put("f l t",    new Coordinate(-52.5, -34.0, this.side)); // flag l t
		//Flags for the left goal
		this.field.put("g l",      new Coordinate(-52.5,   0.0, this.side)); // goal l
		this.field.put("f g l t",  new Coordinate(-52.5,  -7.0, this.side)); // flag g l t
		this.field.put("f p l t",  new Coordinate(-36.0, -20.0, this.side)); // flag p l t
		this.field.put("f p l c",  new Coordinate(-36.0,   0.0, this.side)); // flag p l c
		this.field.put("f p l b",  new Coordinate(-36.0,  20.0, this.side)); // flag p l b
		this.field.put("f g l b",  new Coordinate(-52.5,   7.0, this.side)); // flag g l b
		//Flags for the right goal
		this.field.put("g r",      new Coordinate( 52.5,   0.0, this.side)); // goal l
		this.field.put("f g r t",  new Coordinate( 52.5,  -7.0, this.side)); // flag g l t
		this.field.put("f p r t",  new Coordinate( 36.0, -20.0, this.side)); // flag p l t
		this.field.put("f p r c",  new Coordinate( 36.0,   0.0, this.side)); // flag p l c
		this.field.put("f p r b",  new Coordinate( 36.0,  20.0, this.side)); // flag p l b
		this.field.put("f g r b",  new Coordinate( 52.5,   7.0, this.side)); // flag g l b
		//this.setPriority(Thread.MAX_PRIORITY);
	}
	
	public Position getPosition() {
		return new Position(this.x, this.y, this.bodyAngle, this.neckAngle);
	}
	
	public Position getPositionAtTime(long time) {
		return new Position(this.x, this.y, this.bodyAngle, Self.BUFFER.getNeckAngleAtTime(time));
	}
	
	public void updateXY(double x, double y) {
		this.x = x;
		this.y = y;
		this.init = true;
	}
	
	public boolean isInitiated() {
		return this.init;
	}
	
	public void setGlobalTime(long step) {
		this.step = step;
	}
	
	public long getGlobalTime() {
		return this.step;
	}
	
	public void setFlags(HashMap<String, String> flags, long step) {
		this.lock.lock();
		//System.out.println("step received: " + step);
		if (step > this.lastStepUpdated) {
			this.flags = flags;
//			this.updating = true;
//			this.update.signal();
			this.lastStepUpdated = step;
			this.updateWorld();
			//System.out.println("flags received");
		}
		this.lock.unlock();
	}
	
	private void updateWorld() {
		//System.out.println("updating");
		this.neckAngle = Self.HEAD_ANGLE;
		Set<String> s = this.flags.keySet();
		Iterator<String> i = s.iterator();
		Vector<Double> xs = new Vector<Double>();
		Vector<Double> ys = new Vector<Double>();
		Vector<Double> angles = new Vector<Double>();
		Vector<String> flags = new Vector<String>();
		while(i.hasNext()) {
			flags.add(i.next());
		}
		int limit = 0;
		if (this.accuracy == 0) {
			limit = flags.size();
		} else {
			if (flags.size() > this.accuracy) {
				limit = this.accuracy;
			} else {
				limit = flags.size();
			}
		}
		for (int j = 0; j < limit - 1; j++) {
			for (int k = j + 1; k < limit; k++) {
				Coordinate c0 = this.field.get(flags.elementAt(j));
				Coordinate c1 = this.field.get(flags.elementAt(k));
				String parameter = this.flags.get(flags.elementAt(j));
				String[] parameters = parameter.split(" ");
				double d0 = Double.parseDouble(parameters[0]);
				double alpha0 = Double.parseDouble(parameters[1]); //direction to point0
				parameter = this.flags.get(flags.elementAt(k));
				parameters = parameter.split(" ");
				double d1 = Double.parseDouble(parameters[0]);
				double alpha1 = Double.parseDouble(parameters[1]); //direction to point1
				double k0 = Math.pow(c0.x, 2.0) + Math.pow(c0.y, 2.0) - Math.pow(d0, 2.0);
				double k1 = Math.pow(c1.x, 2.0) + Math.pow(c1.y, 2.0) - Math.pow(d1, 2.0);
				double x0, y0;
				double x1, y1;
				double C, B;
				boolean discriminante = true;
				if (c0.x == c1.x) {
					//System.out.println("same x's");
					double y = (k1 - k0) / (2.0 * (c1.y - c0.y));
					y0 = y;
					y1 = y;
					B = -2.0 * c0.x;
					C = Math.pow(y, 2.0) - 2.0 * y * c0.y + k0;
					if (Math.pow(B, 2.0) - 4 * C < 0) {
						discriminante = false;
					}
					if (discriminante) {
						x0 = (-B + Math.sqrt(Math.pow(B, 2.0) - 4 * C)) / 2.0;
						x1 = (-B - Math.sqrt(Math.pow(B, 2.0) - 4 * C)) / 2.0;
					} else {
						x0 = 0.0;
						x1 = 0.0;
					}
				} else {
					if (c0.y == c1.y) {
						//System.out.println("same y's");
						double x = (k1 - k0) / (2.0 * (c1.x - c0.x));
						x0 = x;
						x1 = x;
						B = -2.0 * c0.y;
						C = Math.pow(x, 2.0) - 2.0 * x * c0.x + k0;
						if (Math.pow(B, 2.0) - 4 * C < 0) {
							discriminante = false;
						}
						if (discriminante) {
							y0 = (-B + Math.sqrt(Math.pow(B, 2.0) - 4 * C)) / 2.0;
							y1 = (-B - Math.sqrt(Math.pow(B, 2.0) - 4 * C)) / 2.0;
						} else {
							y0 = 0.0;
							y1 = 0.0;
						}
					} else {
						//System.out.println("discriminante");
						double M = (k1 - k0) / (2.0 * (c1.x - c0.x));
						double N = (c0.y - c1.y) / (c1.x - c0.x);
						B = (2.0 * (M * N - N * c0.x - c0.y)) / (Math.pow(N, 2.0) + 1.0);
						C = (k0 + Math.pow(M, 2.0) - 2.0 * M * c0.x) / (Math.pow(N, 2.0) + 1.0);
						if (Math.pow(B, 2.0) - 4 * C < 0) {
							discriminante = false;
						}
						if (discriminante) {
							y0 = (-B + Math.sqrt(Math.pow(B, 2.0) - 4 * C)) / 2.0;
							x0 = M + y0 * N;
							y1 = (-B - Math.sqrt(Math.pow(B, 2.0) - 4 * C)) / 2.0;
							x1 = M + y1 * N;
						} else {
							x0 = 0.0;
							y0 = 0.0;
							x1 = 0.0;
							y1 = 0.0;
						}
					}
				}
				//System.out.println(x0 + ", " + y0 + "; " + x1 + ", " + y1);
				if (discriminante) {
					if (checkSolution(c0,c1,x0,y0,x1,y1)) {
						xs.add(x0);
						ys.add(y0);
						double gamma0 = Math.toDegrees(Math.atan2(c0.y - y0,c0.x - x0)) - alpha0;
						if (gamma0 >= 180.0) {
							gamma0 = gamma0 - 360.0;
						}
						if (gamma0 < -180.0) {
							gamma0 = gamma0 + 360.0;
						}
						double gamma1 = Math.toDegrees(Math.atan2(c1.y - y0,c1.x - x0)) - alpha1;
						if (gamma1 >= 180.0) {
							gamma1 = gamma1 - 360.0;
						}
						if (gamma1 < -180.0) {
							gamma1 = gamma1 + 360.0;
						}
						double gamma = this.midPoint(gamma0, gamma1);
						angles.add(gamma);
					} else {
						xs.add(x1);
						ys.add(y1);
						double gamma0 = Math.toDegrees(Math.atan2(c0.y - y1,c0.x - x1)) - alpha0;
						if (gamma0 >= 180.0) {
							gamma0 = gamma0 - 360.0;
						}
						if (gamma0 < -180.0) {
							gamma0 = gamma0 + 360.0;
						}
						double gamma1 = Math.toDegrees(Math.atan2(c1.y - y1,c1.x - x1)) - alpha1;
						if (gamma1 >= 180.0) {
							gamma1 = gamma1 - 360.0;
						}
						if (gamma1 < -180.0) {
							gamma1 = gamma1 + 360.0;
						}
						double gamma = this.midPoint(gamma0, gamma1);
						angles.add(gamma);
					}
				}
			}
		}
		this.x = this.average(xs);
		this.y = this.average(ys);
		if (angles.size() > 0) {
			double gamma = angles.elementAt(0);
			for (int j = 1; j < angles.size(); j++) {
				gamma = this.midPoint(gamma, angles.elementAt(j));
			}
			this.bodyAngle = gamma - this.neckAngle;
		} else {
			this.bodyAngle = 0.0;
		}
		//System.out.println("x: " + Double.toString(this.x) + ", y: " + Double.toString(this.y) + ", theta: " + Double.toString(this.theta));
	}
	
	private boolean checkSolution(Coordinate c0, Coordinate c1, double x0, double y0, double x1, double y1) {
		boolean flag = false;
		double d0 = Math.sqrt(Math.pow(x0 - this.x, 2.0) + Math.pow(y0 - this.y, 2.0));
		double d1 = Math.sqrt(Math.pow(x1 - this.x, 2.0) + Math.pow(y1 - this.y, 2.0));
		if (d0 < d1) {
			flag = true;
		}
		return flag;
	}
	
	private double average(Vector<Double> data) {
		//System.out.println();
		double suma = 0.0;
		double promedio = 0.0;
		for (int i = 0; i < data.size(); i++) {
			suma = suma + data.elementAt(i);
			//System.out.println(Double.toString(data.elementAt(i)));
		}
		if (data.size() > 0) {
			promedio = suma / (double)data.size();
		}
		return promedio;
	}
	
	private double midPoint(double alpha0, double alpha1) {
		double midPoint;
		double midPoint0 = (alpha0 + alpha1) / 2.0;
		double midPoint1 = midPoint0 + 180.0;
		if (midPoint1 >= 180.0) {
			midPoint1 = midPoint1 - 360.0;
		}
		if (this.minArc(alpha0, midPoint0) < this.minArc(alpha0, midPoint1)) {
			midPoint = midPoint0;
		} else {
			midPoint = midPoint1;
		}
		return midPoint;
	}
	
	private double minArc(double angle0, double angle1) { //angle0: source, angle1: objective
		double arc = Math.abs(angle1 - angle0);
		if (arc > 180.0) {
			arc = 360.0 - arc ;
		}
		return arc;
	}

//	public void shutdown() {
//		this.lock.lock();
//		this.running = false;
//		this.closing = true;
//		this.updating = true;
//		this.update.signal();
//		this.lock.unlock();
//	}
//	
//	@Override
//	public void run() {
//		while (this.running) {
//			this.lock.lock();
//			try {
//				while(!this.updating) {
//					this.update.await();
//				}
//			} catch (Exception e) {
//				System.err.println(e);
//			}
//			//Here I update the playerModel
//			if (!this.closing) {
//				this.updateWorld();
//			}
//			//finish the update
//			this.updating = false;
//			this.lock.unlock();
//		}
//		//System.out.println("PlayerModel closed");
//	}
	
	/*Test*/
	
	/*public static void main(String[] args) {
	 
	}/**/
	
}
